﻿using ModelLayer;
using System;
using System.Collections.Generic;
using System.Linq;

namespace ControllerLayer.AStar
{
    public class Graph
    {
        private readonly List<Node> _nodes;
        private readonly List<Edge> _edges;

        public Graph(List<Node> nodesList, IEnumerable<Edge> edgesList)
        {
            _nodes = nodesList;
            _edges = new List<Edge>();
            foreach (Edge edge in edgesList)
            {
                AddEdge(edge.FirstNode.Station, edge.SecondNode.Station, edge.Distance, edge.Time);
            }
        }

        private void AddEdge(Station fromStation, Station toStation, double distance, TimeSpan time)
        {
            var fromNode = _nodes.Single(n => n.Station.Id.Equals(fromStation.Id));
            var toNode = _nodes.Single(n => n.Station.Id.Equals(toStation.Id));
            var edge = new Edge(fromNode, toNode, distance, time);
            _edges.Add(edge);

            fromNode.Edges.Add(edge);
            toNode.Edges.Add(edge);
        }

        /// <summary>
        /// This method returns the direct (birdseye) distance in km between to stations.
        /// </summary>
        /// <param name="currentNode">The current station.</param>
        /// <param name="goalNode">The goal station.</param>
        /// <returns></returns>
        private static double HeuristicEstimate(Node currentNode, Node goalNode)
        {
            /*
             * This uses the ‘haversine’ formula to calculate the great-circle distance between two points –
             * that is, the shortest distance over the earth’s surface – giving an ‘as-the-crow-flies’ distance
             * between the points (ignoring any hills, of course!
             */

            Station currentStation = currentNode.Station;
            Station goalStation = goalNode.Station;

            decimal cSLat = (decimal)(currentStation.Latitude / 100000);
            decimal cSLong = (decimal)(currentStation.Longtitude / 100000);
            decimal gSLat = (decimal)(goalStation.Latitude / 100000);
            decimal gSLong = (decimal)(goalStation.Longtitude / 100000);

            const double radius = 6371; //km radius of earth
            //All angles are converted to radians
            double dLat = Math.PI * (double)((gSLat - cSLat) / 180);
            double dLon = Math.PI * (double)(gSLong - cSLong) / 180;
            double lat1 = Math.PI * (double)(cSLat) / 180;
            double lat2 = Math.PI * (double)(gSLat) / 180;

            double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) + Math.Sin(dLon / 2) *
                Math.Sin(dLon / 2) * Math.Cos(lat1) * Math.Cos(lat2);

            double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));

            double distance = radius * c;

            return distance;
        }

        /// <summary>
        /// Finds the minimum distance path using AStar algorithm.
        /// </summary>
        /// <param name="startStation">The start station.</param>
        /// <param name="endStation">The end station.</param>
        /// <returns></returns>
        public IEnumerable<PartRoute> MinDistancePath(Station startStation, Station endStation)
        {
            var partRoutes = new Stack<PartRoute>();

            //This gets the nodes if a path is found
            IEnumerable<Node> nodes = FindPath(startStation, endStation);
            IEnumerator<Node> node = nodes.GetEnumerator();
            Node startNode = null;
            while (node.MoveNext())
            {
                //Only applies to first run of this loop
                if (startNode == null)
                {
                    startNode = node.Current;
                }
                else
                {
                    Node endNode = startNode;
                    startNode = node.Current;
                    var edge = GetEdge(startNode, endNode);
                    var partRoute = new PartRoute
                       {
                           LocationEnd = endNode.Station,
                           LocationStart = startNode.Station,
                           Distance = edge.Distance,
                           Time = edge.Time
                       };

                    partRoutes.Push(partRoute);
                }
            }
            return partRoutes;
        }

        /// <summary>
        /// The A* algorithm for finding shortest path
        /// </summary>
        /// <param name="startStation">The start.</param>
        /// <param name="endStation">The destination.</param>
        /// <returns></returns>
        private IEnumerable<Node> FindPath(Station startStation, Station endStation)
        {
            //Find the start node using a lambda expression on _nodes
            Node start = _nodes.Single(n => n.Station.Name == startStation.Name);
            //Find the end node using a lambda expression on _nodes
            Node destination = _nodes.Single(n => n.Station.Name == endStation.Name);

            #region delegates that are used in the algorithm - funcDistance & funcEstimate

            Func<Node, Node, double> funcDistance =
                (node1, node2) => _edges.Single(
                    e => e.FirstNode.Equals(node1) && e.SecondNode.Equals(node2)
                    || e.FirstNode.Equals(node2) && e.SecondNode.Equals(node1)).Distance;

            Func<Node, double> funcEstimate = node => HeuristicEstimate(node, destination);

            #endregion delegates that are used in the algorithm - funcDistance & funcEstimate

            int i = 0;
            var closed = new HashSet<Node>();
            var queue = new PriorityQueue<double, Path<Node>>();
            queue.Enqueue(0, new Path<Node>(start));
            while (!queue.IsEmpty)
            {
                Path<Node> path = queue.Dequeue();
                //Find out if the node is already visited
                bool alreadyVisited = closed.Any(n => n.Equals(path.CurrentNode));
                //If not proceed to work with the node
                if (!alreadyVisited)
                {
                    if (path.CurrentNode.Equals(destination))
                    {
                        //The path is complete - return the path
                        return path;
                    }
                    closed.Add(path.CurrentNode);

                    //find all the edges from the node
                    List<Edge> edges = _edges.FindAll(e => e.FirstNode.Equals(path.CurrentNode) || e.SecondNode.Equals(path.CurrentNode));
                    //Set the neigbor nodes from the list of edges
                    path.CurrentNode.SetNeigbors(edges);
                    //neighbours are now set
                    foreach (Node n in path.CurrentNode.Neighbours)
                    {
                        //Check to see if the nodes have already been visited before trying to make a new path
                        //Remove this check if it gives a problem
                        if (!closed.Any(cnode => cnode.Equals(n)))
                        {
                            i++;

                            //Gets the known distance between the two nodes
                            double distance = funcDistance(path.CurrentNode, n);

                            var newPath = path.AddStep(n, distance);

                            //Add the new path to the priority queue with Priority = (total cost + estimate)
                            //Here the funcEstimate is used to get a estimated distance to the end node
                            queue.Enqueue(newPath.TotalCost + funcEstimate(n), newPath);
                        }
                    }
                }
            }
            return null;
        }

        /// <summary>
        /// This method finds an edge between two stations
        /// </summary>
        /// <param name="startNode"></param>
        /// <param name="endNode"></param>
        /// <returns>Edge</returns>
        private Edge GetEdge(Node startNode, Node endNode)
        {
            Edge edge = _edges.Single
                (
                 e =>
                     e.FirstNode.Equals(startNode) && e.SecondNode.Equals(endNode)
                     ||
                     e.FirstNode.Equals(endNode) && e.SecondNode.Equals(startNode)
                );
            return edge;
        }
    }
}